;;; Trust-region-algorithm for optimizations

;; Consider the optimization problem,
;;               min f(x), where x is in R^n.

;; The note uses trust-region algorithm, compared to line-search algorithm.

;;;;; IDEA of trust-region algorithm:
;; (a) start at x0,x1;
;; (b) get model-function and its 2nd-order-part: m(d), m2(d);
;; (c) get trust region (with radius R) of model-function (related |x1-x0| and error-estimation)
;; (d) try to find next point x2 = x1 + d1 in R.
;; (e) loop (b-d).

;;看来自己的想法有缺陷, 需要仔细考虑......


#!r6rs
(library-directories '("." "/home/cl/gits/libs-for-chezscheme"))
(import (linear-algebra)
        (stream)
        (range))

(define (descent-direction? d g)
  (< (vector-dot g d) 1e-8))
;; We only give trust-region algorithm under subspace method
(define (subspace grad f0 ng0 ug0 nd0 ud0 x1 f1 ng1 ug1 Rmin)
  ;; grad-function; x0's info:f0,g0; d0=x1-x0; x1's info:f1,g1;
  ;; nv: norm of v, uv: unit-vector of v.
  ;; Rmin: minimun-distance between two points.
  ;; return: (list nd1 ud1 x2)
  (let* ([dots (map vector-dot
                    ;;unit-vec d0g0  d0g1 g0g1
                    (list       ud0  ud0  ug0)
                    (list       ug0  ug1  ug1))]
         [s Rmin] [ds (scalar-*-vector (- s) ug1)]
         [xs (vector-add x1 ds)] [gs (grad xs)]
         [ngs (vector-norm gs)] [ugs (scalar-*-vector (/ 1 ngs) gs)]
         [g1-by-d0 (/ ng1 nd0)] [g0-by-g1 (/ ng0 ng1)] [gs-by-g1 (/ ngs ng1)] [g1-by-ds (/ ng1 s)]
         [d0Gd0 (* g1-by-d0 (- (list-ref dots 1) (* g0-by-g1 (list-ref dots 0))))]
         [d0Gg1 (* g1-by-d0 (- 1 (* g0-by-g1 (list-ref dots 2))))]
         [g1Gg1 (* g1-by-ds (- 1 (* gs-by-g1 (vector-dot ug1 ugs))))]
         [positive-definitive? (and (positive? d0Gd0)
                                    (positive? (- (* d0Gd0 g1Gg1) (expt d0Gg1 2))))]);;检查G的正定性
    (if positive-definitive?
        (let* ([m-at-x0 (+ f1 (* -1 nd0 ng1 (list-ref dots 1)) (* 1/2 d0Gd0 (expt nd0 2)))]
               [er (let ([aux (abs (/ m-at-x0 f0))])
                     (- 1 (max aux (/ 1 aux))))]
               [R (cond [(< er 0) (* 1/2 nd0)]
                        [(< er 0.5) nd0]
                        (else (* 2 nd0)))])
          (printf "er= ~a, R=~a;\n" er R)
          (let* ([beta (/ (- (* (list-ref dots 1) g1Gg1) d0Gg1)
                          (- (* (list-ref dots 1) d0Gg1) d0Gd0))]
                 [dir (vector-minus (scalar-*-vector beta ud0) ug1)]
                 [ndir (vector-norm dir)] [udir (scalar-*-vector (/ 1 ndir) dir)]
                 [pre-s (/ (- 1 (* beta (list-ref dots 1)))
                           (+ g1Gg1 (* -2 beta d0Gg1) (* beta beta d0Gd0)))])
            (if (descent-direction? udir ug1)
                (if (or (< er 1e-8) (<= (* pre-s ndir) R))
                    (list (abs (* pre-s ndir)) udir
                          (vector-add x1 (scalar-*-vector pre-s dir)));;ok
                    (list R udir
                          (vector-add x1 (scalar-*-vector (/ R ndir) dir))))
                (begin (printf "use ** sd ** ")
                       (list Rmin (scalar-*-vector -1 ug1) xs)))))
        (begin (printf "USE SD ** ")
          (list Rmin (scalar-*-vector -1 ug1) xs)))));;ok

;; Trust-region-algorithm expt
(define (trust-region-algorithm f grad x0 Rmin gmin)
  ;;object-func: f(x),grad, x0: starting point, Rmin: minimum radius
  (let* ([g0 (grad x0)] [ng0 (vector-norm g0)] [ug0 (scalar-*-vector (/ 1 ng0) g0)]
         [nd0 Rmin] [d0 (scalar-*-vector (- nd0) ug0)]
         [x1 (vector-add x0 d0)] [g1 (grad x1)]
         [ng1 (vector-norm g1)] [ug1 (scalar-*-vector (/ 1 ng1) g1)])
    ;;(display x0) (newline)
    ;;(display x1) (newline)
    (let loop ([f0 (f x0)] [ng0 ng0] [ug0 ug0]
               [nd0 nd0] [ud0 (scalar-*-vector -1 ug0)]
               [x1 x1] [f1 (f x1)] [ng1 ng1] [ug1 ug1])
      (let* ([sub (subspace grad f0 ng0 ug0 nd0 ud0 x1 f1 ng1 ug1 Rmin)];;subspace
             [nd1 (car sub)]
             [ud1 (cadr sub)]
             [x2 (caddr sub)])
        ;;(printf "x = ~a\n" x2)
        (if (< ng1 gmin)
            x2  ;;ok
            (let* ([g2 (grad x2)] [ng2 (vector-norm g2)] [ug2 (scalar-*-vector (/ 1 ng2) g2)])
              (loop f1 ng1 ug1 nd1 ud1 x2 (f x2) ng2 ug2)))))
    ))


;;;test
(define (rosenbrock)
  (define (f v)
    (+ (expt (- 1 (vector-ref v 0)) 2)
       (* 100 (expt (- (vector-ref v 1) (expt (vector-ref v 0) 2)) 2))))
  (define (g v)
    (let ([aux (- (vector-ref v 1) (expt (vector-ref v 0) 2))])
      (vector (+ (* -2 (- 1 (vector-ref v 0)))
                 (* -400 (vector-ref v 0) aux))
              (* 200 aux))))
  (define v0 (vector -1.2 0))
  (list f g v0))

(define (example2 n)
  (define G-diag (vector-map (lambda (ele) (+ 0.1 (random 100.)))
                             (vector-range n)))
  (define x0 (vector-map (lambda (ele) (- (random 200.) 100))
                            (vector-range n)))
  (define G (diag G-diag))
  (define (f v)
    (* 1/2 (vector-dot v (matrix-*-vector G v))))
  (define (g v)
    (matrix-*-vector G v))
  (list f g x0))

(let* ([eg (example2 4)];;(rosenbrock)
       [f (car eg)]
       [grad (cadr eg)]
       [x0 (caddr eg)])
  (display (trust-region-algorithm f grad x0 1e-1 1e-2))
  (newline))
(exit)
